#include "bsp/bsp.h"
#include "bsp/bsp_drivers.h"
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim_aux;
#ifdef MAG_ENC_TIMER
TIM_HandleTypeDef htim_enc;
#endif
#ifdef HALL_TIMER
TIM_HandleTypeDef htim_hall;
#endif
TIM_HandleTypeDef htim_sch;
TIM_HandleTypeDef htim_ticks;

bool brake_resistor_armed = false;
static void Error_Handler(void) {}

void mc_sched_timer_init(int ms){
	MC_1MS_TIMER_CLK_ENABLE();

   	TIM_MasterConfigTypeDef sMasterConfig = {0};
   
   	/* USER CODE BEGIN TIM7_Init 1 */
   
   	/* USER CODE END TIM7_Init 1 */
   	htim_sch.Instance = MC_1MS_TIMER;
   	htim_sch.Init.Prescaler = MC_1MS_TIMER_CLKM - 1;
   	htim_sch.Init.CounterMode = TIM_COUNTERMODE_UP;
   	htim_sch.Init.Period = ms - 1;
   	htim_sch.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
   	HAL_TIM_Base_Init(&htim_sch);

	__HAL_TIM_CLEAR_FLAG(&htim_sch, TIM_FLAG_UPDATE);
	__HAL_TIM_ENABLE_IT(&htim_sch, TIM_IT_UPDATE);
    HAL_NVIC_SetPriority(MC_1MS_TIMER_nIRQ, SCHED_TIMER_IRQ_PRIORITY, 0);
    HAL_NVIC_EnableIRQ(MC_1MS_TIMER_nIRQ);

	__HAL_TIM_ENABLE(&htim_sch);
}

#ifdef CONFIG_CPU_TICK_TIMER
void cpu_ticks_timer_init(void){
	CPU_TICK_TIMER_CLK_ENABLE();

   	TIM_MasterConfigTypeDef sMasterConfig = {0};
   
   	htim_ticks.Instance = CONFIG_CPU_TICK_TIMER;
   	htim_ticks.Init.Prescaler = CPU_TICK_TIMER_CLKM - 1;
   	htim_ticks.Init.CounterMode = TIM_COUNTERMODE_UP;
   	htim_ticks.Init.Period = 0xFFFFFFFF; /* 支持32bit的timer，需要设置AAR超过16bit，否则计数器还是16bit模式 */
   	htim_ticks.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
   	HAL_TIM_Base_Init(&htim_ticks);
	__HAL_TIM_ENABLE(&htim_ticks);
	__HAL_TIM_SetCounter(&htim_ticks, 0xFFFFFFFF);
}
#endif

/* 通过传入死区等于多少个TIM clk，计算实际写入register的大小 */
static u8 _dead_time(u16 t) {
	if (t < 128) {
		return (u8 )t;
	}else if (t <= (64 + 63) * 2) { //11 1111
		return ((((u8)2<<6) + (t-64)/2));
	}else if (t <= (32 + 31) * 8) {
		return (((u8)3 << 6) + (t - 32)/8);
	}else {
		if ((t-32)/16 > 63) {
			return 0xFF;
		}
		return (((u8)7<<3) + (t - 32)/16);
	}
}

/** 
 * 需要配置为，break 有效，MOE关闭的时候，首先需要进入的状态是: 关闭上桥，开通下桥
 * 如果是全关闭的话，可能发生高速反电动势过高，经过整流后母线电流太大，迫使电池关闭输出
 * 等到速度低于一定程度，再由软件开启输出，关闭所有mos
*/
void pwm_timer_init(u32 freq)
{
  	TIM_ClockConfigTypeDef sClockSourceConfig;
  	TIM_MasterConfigTypeDef sMasterConfig;
  	TIM_OC_InitTypeDef sConfigOC;
  	TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
	bool enable_brk = true;
	PWM_TIMER_CLK_ENABLE();

  	htim1.Instance = PWM_TIMER;
  	htim1.Init.Prescaler = 0;
  	htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
  	htim1.Init.Period = CONFIG_FOC_PWM_Half_Period;
  	htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  	htim1.Init.RepetitionCounter = CONFIG_FOC_PWM_UPDATE_REPEAT;
  	if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  	if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	if (HAL_TIM_OC_Init(&htim1) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC4REF;
  	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  	if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sConfigOC.OCMode = TIM_OCMODE_PWM1;
  	sConfigOC.Pulse = 0;
  	sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  	sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  	sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  	sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
  	sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET;
  	if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

 	sConfigOC.OCMode = TIM_OCMODE_PWM2;
	sConfigOC.Pulse = CONFIG_FOC_PWM_Half_Period - ADC_TIM_CC4_OFFSET;
  	if (HAL_TIM_OC_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}
#ifdef STM32G4
	TIMEx_BreakInputConfigTypeDef sBreakInputConfig = {0};
	sBreakInputConfig.Source = TIM_BREAKINPUTSOURCE_BKIN;
	sBreakInputConfig.Enable = TIM_BREAKINPUTSOURCE_ENABLE;
	sBreakInputConfig.Polarity = TIM_BREAKINPUTSOURCE_POLARITY_LOW;
	if (HAL_TIMEx_ConfigBreakInput(&htim1, TIM_BREAKINPUT_BRK, &sBreakInputConfig) != HAL_OK)
	{
	  Error_Handler();
	}
#endif
  	sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_ENABLE;
  	sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_ENABLE;
  	sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
  	sBreakDeadTimeConfig.DeadTime = _dead_time(CONFIG_PWM_DEADTIME_nCLKs);
#ifdef PWM_TIMER_BRK_IN_PIO
	sBreakDeadTimeConfig.BreakState = enable_brk?TIM_BREAK_ENABLE:TIM_BREAK_DISABLE;
#else
  	sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
#endif
  	sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_LOW;

#ifdef STM32G4
	sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT;
	sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
	sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
	sBreakDeadTimeConfig.Break2Filter = 0;
	sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT;
#endif
  	sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
  	if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	gpio_af_init(PWM_U_H_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
  	gpio_af_init(PWM_U_L_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
  	gpio_af_init(PWM_V_H_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
  	gpio_af_init(PWM_V_L_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
  	gpio_af_init(PWM_W_H_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
  	gpio_af_init(PWM_W_L_PIO, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);

	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
	__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
	HAL_NVIC_SetPriority(PWM_TIMER_UPDATE_nIRQ, TIMER_UP_IRQ_PRIORITY, 0);
	HAL_NVIC_EnableIRQ(PWM_TIMER_UPDATE_nIRQ);
#ifdef PWM_TIMER_BRK_IN_PIO
	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_BREAK);
	__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_BREAK);
	HAL_NVIC_SetPriority(PWM_TIMER_BRK_nIRQ, PWM_BRAKE_IRQ_PRIORITY, 0);
	HAL_NVIC_EnableIRQ(PWM_TIMER_BRK_nIRQ);
	gpio_af_init(PWM_TIMER_BRK_IN_PIO, GPIO_MODE_AF_OD, GPIO_NOPULL, GPIO_SPEED_LOW, PWM_PIN_AF_FUNC);
#endif
	__HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);

	pwm_disable_channel();

	PWM_ENABLE_ADC_TRIGGER();

	__HAL_TIM_MOE_ENABLE(&htim1);

	__HAL_TIM_SET_COUNTER(&htim1, htim1.Init.Period); //让tim在溢出三角波等点更新ccr，aar等寄存器

	__HAL_TIM_ENABLE(&htim1);
}


void aux_timer_init(void)
{
#ifdef AUX_TIMER
  	TIM_MasterConfigTypeDef sMasterConfig;
  	TIM_OC_InitTypeDef sConfigOC;

  	htim_aux.Instance = AUX_TIMER;
  	htim_aux.Init.Prescaler = 0;
  	htim_aux.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
  	htim_aux.Init.Period = AUX_TIM_PERIOD_CLOCKS;
  	htim_aux.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  	if (HAL_TIM_PWM_Init(&htim_aux) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  	if (HAL_TIMEx_MasterConfigSynchronization(&htim_aux, &sMasterConfig) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sConfigOC.OCMode = TIM_OCMODE_PWM2;
  	sConfigOC.Pulse = 0;
  	sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
  	sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  	if (HAL_TIM_PWM_ConfigChannel(&htim_aux, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

  	sConfigOC.Pulse = AUX_TIM_PERIOD_CLOCKS+1;
  	sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  	if (HAL_TIM_PWM_ConfigChannel(&htim_aux, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}

	gpio_af_init(AUX_PWM_L_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, GPIO_AF1_TIM2);
  	gpio_af_init(AUX_PWM_H_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, GPIO_AF1_TIM2);
	__HAL_TIM_ENABLE(&htim_aux);
#endif
}

void encoder_abi_timer_init(u32 max_res)
{
#ifdef MAG_ENC_TIMER
  	TIM_Encoder_InitTypeDef sConfig;
  	TIM_MasterConfigTypeDef sMasterConfig;

	MAG_ENC_TIMER_CLK_ENABLE();
  	htim_enc.Instance = MAG_ENC_TIMER;
  	htim_enc.Init.Prescaler = 0;
  	htim_enc.Init.CounterMode = TIM_COUNTERMODE_UP;
  	htim_enc.Init.Period = max_res - 1;
  	htim_enc.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  	sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
  	sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
  	sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  	sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  	sConfig.IC1Filter = MAG_TIMER_FILTER_VALUE;
  	sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
  	sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  	sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  	sConfig.IC2Filter = MAG_TIMER_FILTER_VALUE;
  	if (HAL_TIM_Encoder_Init(&htim_enc, &sConfig) != HAL_OK)
  	{
//    	_Error_Handler(__FILE__, __LINE__);
  	}

  	sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  	if (HAL_TIMEx_MasterConfigSynchronization(&htim_enc, &sMasterConfig) != HAL_OK)
  	{
    	//_Error_Handler(__FILE__, __LINE__);
  	}
	gpio_af_init(MAG_ENC_PIN_A, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, GPIO_AF2_TIM3);
	gpio_af_init(MAG_ENC_PIN_B, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, GPIO_AF2_TIM3);
#endif

#ifdef MAG_ENC_Z_TIMER
#if CONFIG_ENCODER_ABI
	TIM_IC_InitTypeDef sConfigIC = {0};
	sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
	sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
	sConfigIC.ICFilter = 0;
	if (HAL_TIM_IC_ConfigChannel(&htim_enc, &sConfigIC, MAG_ENC_Z_TIMER_CH) != HAL_OK)
	{
	  	//_Error_Handler(__FILE__, __LINE__);
	}
	gpio_af_init(MAG_ENC_PIN_Z, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_LOW, GPIO_AF2_TIM3);
	__HAL_TIM_CLEAR_FLAG(&htim_enc, MAG_ENC_Z_IRQ_FLAGS);
	__HAL_TIM_ENABLE_IT(&htim_enc, MAG_ENC_Z_IRQ_INT);
	HAL_NVIC_SetPriority(MAG_ENC_Z_nIRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);
	HAL_NVIC_EnableIRQ(MAG_ENC_Z_nIRQ);
	TIM_CCxChannelCmd(htim_enc.Instance, MAG_ENC_Z_TIMER_CH, TIM_CCx_ENABLE);
#endif
#endif
#ifdef MAG_ENC_TIMER
	__HAL_TIM_ENABLE(&htim_enc);
#endif
}


void pwm_start_output(void) {
	pwm_update_duty(0, 0, 0);
	pwm_update_adc_trigger(htim1.Init.Period - ADC_TIM_CC4_OFFSET);
	u32 mask = cpu_enter_critical();
	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
	LL_TIM_GenerateEvent_UPDATE(htim1.Instance);
	while(!__HAL_TIM_GET_FLAG(&htim1, TIM_FLAG_UPDATE));
	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
	pwm_enable_channel();
	cpu_udelay(100);
	cpu_exit_critical(mask);
#ifdef AUX_TIMER
	/* start aux timer pwm channel */
    htim_aux.Instance->CCR3 = 0;
    htim_aux.Instance->CCR4 = AUX_TIM_PERIOD_CLOCKS + 1;
    HAL_TIM_PWM_Start(&htim_aux, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim_aux, TIM_CHANNEL_4);
	brake_resistor_armed = true;
#endif
}


void pwm_stop_output(void) {
	__HAL_TIM_MOE_DISABLE(&htim1);
	pwm_disable_channel();
	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
#ifdef AUX_TIMER
    htim_aux.Instance->CCR3 = 0;
    htim_aux.Instance->CCR4 = AUX_TIM_PERIOD_CLOCKS + 1;
    HAL_TIM_PWM_Stop(&htim_aux, TIM_CHANNEL_3);
    HAL_TIM_PWM_Stop(&htim_aux, TIM_CHANNEL_4);
	brake_resistor_armed = false;
#endif
}

void pwm_enable_update_irq(bool en) {
    if (en) {
		__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
	}else {
		__HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
	}
}

